/*
Copyright (C) 2019  JingWeiZhangHuai
This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with this program.  If not, see <https://www.gnu.org/licenses/>.
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>

#include "morn_math.h"

float morn_gaussian_curve_LUT[400] = { 
0.398942280f,0.398922334f,0.398862500f,0.398762797f,0.398623254f,0.398443914f,0.398224830f,0.397966068f,0.397667706f,0.397329832f,
0.396952548f,0.396535966f,0.396080212f,0.395585421f,0.395051741f,0.394479331f,0.393868362f,0.393219015f,0.392531483f,0.391805971f,
0.391042694f,0.390241878f,0.389403759f,0.388528585f,0.387616615f,0.386668117f,0.385683369f,0.384662661f,0.383606292f,0.382514571f,
0.381387816f,0.380226355f,0.379030526f,0.377800677f,0.376537162f,0.375240347f,0.373910605f,0.372548319f,0.371153879f,0.369727684f,
0.368270140f,0.366781662f,0.365262673f,0.363713600f,0.362134882f,0.360526963f,0.358890291f,0.357225325f,0.355532529f,0.353812371f,
0.352065327f,0.350291879f,0.348492513f,0.346667721f,0.344818001f,0.342943855f,0.341045789f,0.339124313f,0.337179944f,0.335213200f,
0.333224603f,0.331214680f,0.329183961f,0.327132977f,0.325062264f,0.322972360f,0.320863804f,0.318737139f,0.316592908f,0.314431657f,
0.312253933f,0.310060285f,0.307851261f,0.305627410f,0.303389284f,0.301137432f,0.298872406f,0.296594755f,0.294305030f,0.292003780f,
0.289691553f,0.287368897f,0.285036359f,0.282694482f,0.280343811f,0.277984886f,0.275618247f,0.273244431f,0.270863972f,0.268477402f,
0.266085250f,0.263688042f,0.261286301f,0.258880547f,0.256471294f,0.254059057f,0.251644341f,0.249227653f,0.246809491f,0.244390351f,
0.241970725f,0.239551098f,0.237131952f,0.234713764f,0.232297005f,0.229882141f,0.227469633f,0.225059935f,0.222653499f,0.220250767f,
0.217852177f,0.215458162f,0.213069147f,0.210685552f,0.208307790f,0.205936269f,0.203571388f,0.201213543f,0.198863119f,0.196520499f,
0.194186055f,0.191860155f,0.189543158f,0.187235418f,0.184937281f,0.182649085f,0.180371163f,0.178103839f,0.175847430f,0.173602247f,
0.171368592f,0.169146761f,0.166937042f,0.164739715f,0.162555055f,0.160383327f,0.158224790f,0.156079696f,0.153948287f,0.151830800f,
0.149727466f,0.147638504f,0.145564130f,0.143504551f,0.141459965f,0.139430566f,0.137416539f,0.135418062f,0.133435304f,0.131468430f,
0.129517596f,0.127582951f,0.125664637f,0.123762790f,0.121877537f,0.120009001f,0.118157295f,0.116322528f,0.114504800f,0.112704207f,
0.110920835f,0.109154766f,0.107406075f,0.105674831f,0.103961095f,0.102264925f,0.100586368f,0.098925471f,0.097282269f,0.095656796f,
0.094049077f,0.092459133f,0.090886979f,0.089332624f,0.087796071f,0.086277319f,0.084776361f,0.083293186f,0.081827776f,0.080380109f,
0.078950158f,0.077537892f,0.076143274f,0.074766262f,0.073406813f,0.072064874f,0.070740394f,0.069433312f,0.068143566f,0.066871091f,
0.065615815f,0.064377664f,0.063156561f,0.061952425f,0.060765169f,0.059594706f,0.058440944f,0.057303789f,0.056183142f,0.055078902f,
0.053990967f,0.052919228f,0.051863577f,0.050823902f,0.049800088f,0.048792019f,0.047799575f,0.046822635f,0.045861076f,0.044914772f,
0.043983596f,0.043067418f,0.042166107f,0.041279530f,0.040407554f,0.039550042f,0.038706856f,0.037877859f,0.037062910f,0.036261869f,
0.035474593f,0.034700939f,0.033940763f,0.033193921f,0.032460266f,0.031739652f,0.031031932f,0.030336959f,0.029654585f,0.028984661f,
0.028327038f,0.027681567f,0.027048100f,0.026426486f,0.025816576f,0.025218220f,0.024631269f,0.024055574f,0.023490985f,0.022937354f,
0.022394530f,0.021862367f,0.021340715f,0.020829427f,0.020328356f,0.019837354f,0.019356277f,0.018884977f,0.018423311f,0.017971133f,
0.017528301f,0.017094671f,0.016670101f,0.016254451f,0.015847579f,0.015449347f,0.015059616f,0.014678249f,0.014305109f,0.013940061f,
0.013582969f,0.013233702f,0.012892126f,0.012558111f,0.012231526f,0.011912244f,0.011600135f,0.011295075f,0.010996937f,0.010705598f,
0.010420935f,0.010142827f,0.009871154f,0.009605797f,0.009346638f,0.009093563f,0.008846454f,0.008605201f,0.008369689f,0.008139809f,
0.007915452f,0.007696508f,0.007482873f,0.007274439f,0.007071105f,0.006872767f,0.006679324f,0.006490676f,0.006306726f,0.006127377f,
0.005952532f,0.005782099f,0.005615984f,0.005454095f,0.005296344f,0.005142641f,0.004992899f,0.004847033f,0.004704958f,0.004566590f,
0.004431848f,0.004300653f,0.004172923f,0.004048582f,0.003927554f,0.003809762f,0.003695134f,0.003583596f,0.003475077f,0.003369508f,
0.003266819f,0.003166943f,0.003069813f,0.002975365f,0.002883534f,0.002794258f,0.002707476f,0.002623126f,0.002541150f,0.002461490f,
0.002384088f,0.002308890f,0.002235839f,0.002164884f,0.002095971f,0.002029048f,0.001964066f,0.001900975f,0.001839726f,0.001780273f,
0.001722569f,0.001666569f,0.001612228f,0.001559502f,0.001508351f,0.001458731f,0.001410602f,0.001363925f,0.001318661f,0.001274771f,
0.001232219f,0.001190968f,0.001150983f,0.001112230f,0.001074673f,0.001038281f,0.001003021f,0.000968862f,0.000935772f,0.000903722f,
0.000872683f,0.000842625f,0.000813521f,0.000785344f,0.000758067f,0.000731665f,0.000706111f,0.000681381f,0.000657452f,0.000634300f,
0.000611902f,0.000590236f,0.000569280f,0.000549013f,0.000529415f,0.000510465f,0.000492144f,0.000474434f,0.000457315f,0.000440770f,
0.000424780f,0.000409330f,0.000394403f,0.000379981f,0.000366051f,0.000352596f,0.000339601f,0.000327053f,0.000314937f,0.000303239f,
0.000291947f,0.000281047f,0.000270527f,0.000260375f,0.000250578f,0.000241127f,0.000232008f,0.000223212f,0.000214728f,0.000206546f,
0.000198656f,0.000191048f,0.000183713f,0.000176642f,0.000169826f,0.000163256f,0.000156926f,0.000150825f,0.000144948f,0.000139285f};

float mGaussianProb(float x,float mean,float stdev)
{
    mException((stdev <= 0.0f),EXIT,"invalid input");
    
    x = (x-mean)/stdev;x = ABS(x);
    
    if(x>3.999f) return 0.0001f;
    
    x = x*100.0f;
    int x1 = (int)x;        int x2 = x1+1;
    float w2 = x-(float)x1; float w1 = 1.0f-w2;
    float prob = w1*morn_gaussian_curve_LUT[x1]+w2*morn_gaussian_curve_LUT[x2];
}

static float morn_basic_v = 0.0f;
static int morn_vector_num = 0;
static float morn_cov_det = 0.0f;

float VecGaussianProb(MVector *x,MVector *mean,MMatrix *inv_cov,float cov_det)
{
    mException((INVALID_VEC(x))||(INVALID_VEC(mean))||(x->size != mean->size),EXIT,"invalid input");
    mException((INVALID_MAT(inv_cov)),EXIT,"invalid input");    
    
    int num = x->size;
    MVector *buff1 = mVectorCreate(num,NULL);
    MVector *buff2 = mVectorCreate(num,NULL);
    
    float v;
    if((morn_vector_num == num)&&(morn_cov_det == cov_det))
    {
        for(int i=0;i<num;i++)
            buff1->data[i] = x->data[i] - mean->data[i];
        v = morn_basic_v;
    }        
    else
    {
        v = cov_det;
        for(int i=0;i<num;i++)
        {
            buff1->data[i] = x->data[i] - mean->data[i];
            v = (v+v)*MORN_PI;
        }
        v = (float)sqrt(v);
        morn_vector_num = num;
        morn_cov_det = cov_det;
        morn_basic_v = v;
    }
    
    mVectorMatrixMul(buff1,inv_cov,buff2);
    float l = mVectorMul(buff2,buff1);
    
    float prob = ((float)exp(l*(-0.5)))/v;
    
    if(prob < 0.0000000001f) prob = 0.0000000001f;
    
    mVectorRelease(buff1);
    mVectorRelease(buff2);
    
    return prob;
}

float mVecGaussianProb(MVector *x,MVector *mean,MMatrix *cov)
{
    mException(INVALID_MAT(cov),EXIT,"invalid input");
    
    MMatrix *inv_cov=mMatrixCreate(cov->row,cov->row,NULL);
    mMatrixInverse(cov,inv_cov);                          //计算协方差的逆
    
    float cov_det = mMatrixDetValue(cov);                           //计算协方差矩阵的行列式
        
    float prob = VecGaussianProb(x,mean,inv_cov,cov_det);
    
    mMatrixRelease(inv_cov);
    
    return prob;
}
